/**
 * 
 */
package program;

import javax.vecmath.Point2d;

import robot.Robot;
import simbad.sim.RangeSensorBelt;

/**
 * @date 7 maj 2012
 * @version 1.0
 */
public class MovingSensorBelt {
	private RangeSensorBelt sensors;
	private MovingObjects movingObjects;
	private Robot self;
	
	/**
	 * @param mo
	 * @param robot
	 */
	public MovingSensorBelt(MovingObjects mo, Robot robot, RangeSensorBelt rsb) {
		this.self = robot;
		this.movingObjects = mo;
		this.sensors = rsb;
	}

	public Point2d getPos (int num) {
		double distance = sensors.getMeasurement(num);
		if (!Double.isInfinite(distance)){
			double angle  = sensors.getSensorAngle(num);
			Point2d pos = movingObjects.getPosCloseTo(getPosFrom(angle, distance));
			return self.getCurrentPosition().equals(pos) ? null : pos;
			
		}
		return null;
	}
	
	public Point2d getDir (int num) {
		Point2d pos = getPos(num);
		if (pos != null) {
			return movingObjects.getDirCloseTo(pos);
		}
		return null;
	}
	
	private Point2d getPosFrom (double angle, double distance) {
		angle += self.getHeading();
		Point2d pos = self.getCurrentPosition();
		return new Point2d ( Math.cos(angle) 
								* distance
								+ pos.x,
							-Math.sin(angle) 
								* distance
								+ pos.y);
	}
}
